-
Notifications
You must be signed in to change notification settings - Fork 646
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
feat(autonomous_emergency_braking): calculate the object's velocity in the search area #8591
feat(autonomous_emergency_braking): calculate the object's velocity in the search area #8591
Conversation
Thank you for contributing to the Autoware project! 🚧 If your pull request is in progress, switch it to draft mode. Please ensure:
|
Hi @danielsanchezaran -san, |
Codecov ReportAttention: Patch coverage is
Additional details and impacted files@@ Coverage Diff @@
## main #8591 +/- ##
==========================================
- Coverage 28.44% 28.44% -0.01%
==========================================
Files 1314 1315 +1
Lines 98290 98340 +50
Branches 39986 40011 +25
==========================================
+ Hits 27958 27971 +13
- Misses 70296 70332 +36
- Partials 36 37 +1
*This pull request uses carry forward flags. Click here to find out more. ☔ View full report in Codecov by Sentry. |
@ismetatabay |
@@ -392,7 +394,9 @@ class AEB : public rclcpp::Node | |||
* @param closest_object Data of the closest object |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@ismetatabay
Could you modify doxygen explanation?
bool is_point_inside = false; | ||
for (const auto & ego_poly : ego_polys) { | ||
if (bg::within(obj_point, ego_poly)) { | ||
is_point_inside = true; | ||
break; | ||
} | ||
} | ||
if (!is_point_inside) { | ||
return false; | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
bool is_point_inside = false; | |
for (const auto & ego_poly : ego_polys) { | |
if (bg::within(obj_point, ego_poly)) { | |
is_point_inside = true; | |
break; | |
} | |
} | |
if (!is_point_inside) { | |
return false; | |
} | |
if (!std::any_of(ego_polys.begin(), ego_polys.end(), | |
[&obj_point](const auto & ego_poly) { return bg::within(obj_point, ego_poly); })) { | |
return false; | |
} |
It might be a matter of preference, but wouldn't this approach be more simple for what you're trying to accomplish? There's no problem with the current implementation, so it's fine if you don't change it.
@ismetatabay |
Hi @kyoichi-sugahara-san, thank you so much for your comments. If you could continue reviewing, I would be grateful. I converted it to a draft as you mentioned a problem with the usage of |
@ismetatabay I can confirm from the description that the results have improved, but do you understand why setting the collision result to false when the object's vertices are outside the extended predicted path helps avoid unnecessary activations? Furthermore, if we were to incorporate this change, I think it would be more natural to implement the process of excluding objects that have vertices not contained within the predicted path in the The reason is that it's preferable to separate the filtering process for potential collision objects from the actual collision detection. I apologize if I've overlooked any circumstances and my comments are unreasonable 🙇 |
@kyoichi-sugahara -san, thank you for your comments. I plan to refactor my implementation and keep this PR as a draft. My current implementation steps are as follows: -> Use rough point cloud filtering to keep the velocity information of the closest object in this area. As you mentioned, the |
95837c9
to
d9d3164
Compare
@kyoichi-sugahara -san, my current idea is:
What do you think? Additionally, I found a mistake in the control.launch.xml launch file and I fixed this topic as well.:
|
So, does this mean that AEB will no longer explicitly react to objects existing outside the normal predicted path? From referring to the issue (this is super useful result ❤️), I understand that the problem is that the estimation results are unstable for the first few steps after speed calculation begins, causing unnecessary activation. From my perspective, I feel the following approaches might be valid solutions:
What do you think? |
@kyoichi-sugahara -san, thank you for your comment ❤️ . So, does this mean that AEB will no longer explicitly react to objects existing outside the normal predicted path? AEB maintains the same mechanism for reacting to obstacles in the predicted path. The default current implementation uses My idea is to calculate the object's velocity when it enters the - Introduce a mechanism that doesn't utilize the first few steps when the speed estimation is clearly unstable.
- Prepare separate areas for speed estimation and for detection targets.
|
Yes you are right, and sorry I noticed my comment was based on misunderstanding
I understand your concern but, may be the further object can be ignored in my opinion. Only closest object in the predicted path polygon should be considered.
yes, that's the concern for this idea which I was discussing with @danielsanchezaran today
Thanks! I hope it improves the behavior but still |
@kyoichi-sugahara -san, Thank you so much for your valuable comments and suggestions. My main concern regarding the closest object is that if we calculate and only consider the closest object within separate areas, we might miss the actual target object. I will work on addressing this in the following case: Since NPC1 is the closest object, we will calculate its speed, but it is not our target object. NPC2 is the target object because it is located on our trajectory. I will check on it. |
d2081c2
to
f463428
Compare
7f428ab
to
7b85937
Compare
} | ||
} | ||
|
||
// If no target object is found, find the closest object overall |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
// If no target object is found, find the closest object overall | |
// If no target object is found, find the closest object overall |
Sorry for the delay in the reviewing @ismetatabay I am finishing some thing regarding AEB ci/cd and it has taken me a little longer than expected to check for degradation, it should be done by tomorrow, on the mean time could you check this? I think the
const auto closest_object_point_itr
calculation part can be deleted, because closest_target_object_itr
already returns the closest object even if there are no targets. So i suggest renaming closest_target_object_itr
to closest_object_itr
and deleting the rest of the redundant code.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
No worries, let me check it 👍
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Removed 👍
@ismetatabay it looks like there might be some trouble with using arc coordinates. I cannot really tell why, but detection seems to happen when it should not. I am using the default params. cap-.2024-09-19-18-54-28.online-video-cutter.com.mp4 |
Hello @danielsanchezaran -san, I couldn't reproduce the issue you encountered with the planning simulator. If possible, could you share the bag and map files? so I can check the problem 2024-09-19.13-44-04.mp4 |
@danielsanchezaran |
7b85937
to
329b4f7
Compare
@ismetatabay thank you for your input. I will try and see if I can reproduce the problem reliably or if it is a tuning problem. The ground segmentation does indeed look suspicious, but I also tried setting the minimum cluster height to 0.1 m to mitigate that, with no luck. Would it be possible to you to do some tests using AWsim? or maybe with a rosbag you have with similar conditions (vehicles close to the ego on both sides) |
@ismetatabay I discovered what was the issue. Some of the MPC path points are virtually the same when the ego is starting to move, that causes the calcLatoffset function to misbehave and not caluclate the proper lateral distance. To solve the issue, we can omit mpc path points that are too close to each other:
I would like you to please add this change and also make the minimum distance between points 1e-2 for the IMU path, for consistency. Also, please note that calcLateralOffset might return NaN, so please handle that case by adding a As a note, the calcLateralOffset function checks if 2 points are too close, but the epsilon value used (1e-8 might be too small. I hopefully will make a PR next week to fix it). On the mean time, I will approve this PR, I think it is a great addition to AEB and I am grateful for it, but please fix the issues I pointed out before merging!!!! |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Approved, but please fix the issues on my last comment before merging, the rest LGTM
329b4f7
to
eae0170
Compare
@danielsanchezaran Thank you so much for your efforts. Before merging it, I want to test it on a real vehicle as well. I will update you. Have a good weekend! |
Thank you @ismetatabay any idea when you will you be able to do the tests? |
eae0170
to
f85fa01
Compare
path.push_back(map_pose); | ||
|
||
if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { | ||
break; | ||
} | ||
} | ||
time_keeper_->end_track("createPath"); | ||
if (path.empty()) return std::nullopt; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@danielsanchezaran Hello, I tested it today and things works as expected in lane_driving scenario. However in parking scenario, sometimes the trajectory will be very short and the returned path will be empty. To fix it I pushed this commit: f85fa01
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@ismetatabay thank you, that is great. So I understand you tested in on the real vehicle right?
PD: I added a small recommendation regarding this line.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@danielsanchezaran Yes, I tested it with speed_calculation_expansion_margin
set to 4.0, and we didn’t stop for any overtaken NPCs during normal lane driving in a campus environment, even though we stopped for a static obstacle at 30 km/h (with -5.0 m/s² emergency deceleration and obstacle_cruise_planner
disabled during the obstacle stop test). Also I pushed your suggestion 👍 65f869b . If it is okay for you, I think we can merge it.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@ismetatabay thank you, it looks great, any chance you might update the discussion about AEB with the new tests ?https://github.com/orgs/autowarefoundation/discussions/5020#discussioncomment-10312780
I have merged the PR, thank you for such a good contribution!
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@danielsanchezaran Thank you as well. Our research vehicle is undergoing a hardware upgrade, and I will likely update the report next week. 👍
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I believe this is more concise
path.push_back(map_pose); | ||
|
||
if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { | ||
break; | ||
} | ||
} | ||
time_keeper_->end_track("createPath"); | ||
if (path.empty()) return std::nullopt; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
if (path.empty()) return std::nullopt; | |
return (!path.empty()) ? std::make_optional(path) : std::nullopt; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Done 👍
f85fa01
to
65f869b
Compare
Signed-off-by: ismetatabay <[email protected]>
Signed-off-by: ismetatabay <[email protected]>
Signed-off-by: ismetatabay <[email protected]>
Signed-off-by: ismetatabay <[email protected]>
Signed-off-by: ismetatabay <[email protected]>
Signed-off-by: ismetatabay <[email protected]>
Signed-off-by: ismetatabay <[email protected]>
Signed-off-by: ismetatabay <[email protected]>
65f869b
to
b49a8ae
Compare
8f0e511
into
autowarefoundation:main
…n the search area (autowarefoundation#8591) * refactor PR Signed-off-by: ismetatabay <[email protected]> * WIP Signed-off-by: ismetatabay <[email protected]> * change using polygon to lateral offset Signed-off-by: ismetatabay <[email protected]> * improve code Signed-off-by: ismetatabay <[email protected]> * remove redundant code Signed-off-by: ismetatabay <[email protected]> * skip close points in MPC path generation Signed-off-by: ismetatabay <[email protected]> * fix empty path points in short parking scenario Signed-off-by: ismetatabay <[email protected]> * fix readme conflicts Signed-off-by: ismetatabay <[email protected]> --------- Signed-off-by: ismetatabay <[email protected]>
…n the search area (autowarefoundation#8591) * refactor PR Signed-off-by: ismetatabay <[email protected]> * WIP Signed-off-by: ismetatabay <[email protected]> * change using polygon to lateral offset Signed-off-by: ismetatabay <[email protected]> * improve code Signed-off-by: ismetatabay <[email protected]> * remove redundant code Signed-off-by: ismetatabay <[email protected]> * skip close points in MPC path generation Signed-off-by: ismetatabay <[email protected]> * fix empty path points in short parking scenario Signed-off-by: ismetatabay <[email protected]> * fix readme conflicts Signed-off-by: ismetatabay <[email protected]> --------- Signed-off-by: ismetatabay <[email protected]> Signed-off-by: prakash-kannaiah <[email protected]>
…n the search area (autowarefoundation#8591) * refactor PR Signed-off-by: ismetatabay <[email protected]> * WIP Signed-off-by: ismetatabay <[email protected]> * change using polygon to lateral offset Signed-off-by: ismetatabay <[email protected]> * improve code Signed-off-by: ismetatabay <[email protected]> * remove redundant code Signed-off-by: ismetatabay <[email protected]> * skip close points in MPC path generation Signed-off-by: ismetatabay <[email protected]> * fix empty path points in short parking scenario Signed-off-by: ismetatabay <[email protected]> * fix readme conflicts Signed-off-by: ismetatabay <[email protected]> --------- Signed-off-by: ismetatabay <[email protected]>
Description
This PR enables the calculation of object velocity within the speed calculation area, using the
speed_calculation_expansion_margin
parameter.Related links
Parent Issue:
How was this PR tested?
Psim
Before This PR:
before_pr_new.mp4
After This PR:
after_pr_new.mp4
Stopping for an obstacle. (obstacle_cruise_planner (motion_stop_planner) is disabled)
after_pr2_new.mp4
Notes for reviewers
None.
Interface changes
None.
ROS Parameter Changes
Additions and removals
speed_calculation_expansion_margin
double
0.5
Effects on system behavior
None.